
#include "slam_ros/occupancy_grid_node.h"

namespace slam_ros {

OccupancyGridNode::OccupancyGridNode(const double resolution, 
      const double publish_period_sec, 
      const std::string map_topic_name,
      const bool include_frozen_submaps,
      const bool include_unfrozen_submaps)
    : Node("slam_occupancy_grid_node"), 
      resolution_(resolution),
      map_topic_name_(map_topic_name),
      include_frozen_submaps_(include_frozen_submaps),
      include_unfrozen_submaps_(include_unfrozen_submaps)
{
  client_ = this->create_client<slam_ros_msgs::srv::SubmapQuery>(kSubmapQueryServiceName);

  occupancy_grid_publisher_ = this->create_publisher<::nav_msgs::msg::OccupancyGrid>(
      kOccupancyGridTopic, rclcpp::QoS(10).transient_local());

  occupancy_grid_publisher_timer_ = this->create_wall_timer(
    std::chrono::milliseconds(int(publish_period_sec * 1000)),
    [this]() {
      DrawAndPublish();
    });

  submap_list_subscriber_ = create_subscription<slam_ros_msgs::msg::SubmapList>(
      kSubmapListTopic, rclcpp::QoS(10), 
      std::bind(&OccupancyGridNode::HandleSubmapList, this, std::placeholders::_1));
}

void OccupancyGridNode::HandleSubmapList(
    const typename slam_ros_msgs::msg::SubmapList::SharedPtr msg) {
  absl::MutexLock locker(&mutex_);

  // We do not do any work if nobody listens.
  if (this->count_publishers(kSubmapListTopic) == 0){
    RCLCPP_WARN(this->get_logger(), "topic(/submap_list) is not found");
    return;
  }

  this->get_parameter_or<int>("localization", slam_type_, 0);
  RCLCPP_INFO_STREAM(get_logger(), "slam_type_:" << slam_type_);
  if(slam_type_ == 1) { return; }

  // Keep track of submap IDs that don't appear in the message anymore.
  std::set<SubmapId> submap_ids_to_delete;
  for (const auto& pair : submap_slices_) {
    submap_ids_to_delete.insert(pair.first);
  }

  for (const auto& submap_msg : msg->submap) {
    const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
    submap_ids_to_delete.erase(id);
    if ((submap_msg.is_frozen && !include_frozen_submaps_) ||
        (!submap_msg.is_frozen && !include_unfrozen_submaps_)) {
      continue;
    }
    SubmapSlice& submap_slice = submap_slices_[id];
    submap_slice.pose = ToRigid3d(submap_msg.pose);
    submap_slice.metadata_version = submap_msg.submap_version;
    if (submap_slice.surface != nullptr &&
        submap_slice.version == submap_msg.submap_version) {
      continue;
    }

    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
    }

    auto srv_request = std::make_shared<slam_ros_msgs::srv::SubmapQuery::Request>();
    srv_request->trajectory_id = id.trajectory_id;
    srv_request->submap_index = id.submap_index;

    using ServiceResponseFuture =
      ::rclcpp::Client<slam_ros_msgs::srv::SubmapQuery>::SharedFuture;
    auto response_received_callback = [&submap_slice](ServiceResponseFuture future) {
      auto fetched_textures = slam_ros::FetchSubmapTextures(future.get());
      if (fetched_textures == nullptr) {
        return;
      }
      CHECK(!fetched_textures->textures.empty());
      submap_slice.version = fetched_textures->version;

      // We use the first texture only. By convention this is the highest
      // resolution texture and that is the one we want to use to construct the
      // map for ROS.
      const auto fetched_texture = fetched_textures->textures.begin();
      submap_slice.width = fetched_texture->width;
      submap_slice.height = fetched_texture->height;
      submap_slice.slice_pose = fetched_texture->slice_pose;
      submap_slice.resolution = fetched_texture->resolution;
      submap_slice.cairo_data.clear();
      submap_slice.surface =  ::slam::io::DrawTexture(
          fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
          fetched_texture->width, fetched_texture->height,
          &submap_slice.cairo_data);
    };

    auto future_result = client_->async_send_request(srv_request, response_received_callback);
  }

  // Delete all submaps that didn't appear in the message.
  for (const auto& id : submap_ids_to_delete) {
    submap_slices_.erase(id);
  }

  last_timestamp_ = msg->header.stamp;
  last_frame_id_ = msg->header.frame_id;
}

void OccupancyGridNode::DrawAndPublish(void) {
  absl::MutexLock locker(&mutex_);
  if (submap_slices_.empty() || last_frame_id_.empty()) {
    return;
  }
  if(slam_type_ == 1) { return; }
  auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_);
  std::unique_ptr<nav_msgs::msg::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
      painted_slices, resolution_, last_frame_id_, last_timestamp_);
  occupancy_grid_publisher_->publish(*msg_ptr);
}

}